/*
 * @Author: ssyyw 2294779310@qq.com
 * @Date: 2024-03-28 23:01:44
 * @LastEditors: ssyyw 2294779310@qq.com
 * @LastEditTime: 2024-03-29 00:20:23
 * @FilePath: \Work\mavproxy\ws_ros1\src\mavproxy_ros1\src\drone_mode_controller.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include "mavproxy_ros1/FlightCmd.h"
#include "mavproxy_ros1/DroneTargetMode.h"
#include "drone_defs.h"

string target_mode = "";

mavros_msgs::State current_state;
mavproxy_ros1::DroneTargetMode target_mode;

void StateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

void ModeCallback(const mavproxy_ros1::DroneTargetMode::ConstPtr &msg)
{
    target_mode = *msg;
}

void OffboardModeLoop()
{
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    // send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while (ros::ok())
    {
        if (current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if (arming_client.call(arm_cmd) &&
                    arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        ros::spinOnce();
        rate.sleep();
    }
}

void InitNode(){
    target_mode.modetype = current_state.mode;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "drone_mode_controller");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>(MAVROS_INFO_STATE, 10, StateCallback);
    ros::Subscriber mode_sub = nh.subscribe<mavproxy_ros1::State>(MAVPROXY_TARGET_MODE, 10, ModeCallback);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    
    InitNode();

    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while (ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    while(ros::ok()){
        
    }

    return 0;
}
